#include "myInit.h"

extern uint8_t gAim_Sign;

/* 运球任务 ***************************************************************/
uint8_t gDribble_OneCount = 0;

float gClawAimAngle = 0; 
uint32_t gClawDeathWaitTime = 0;
void dribbleTask(void *argument)
{
	static enum DribbleMode sDribbleCMD = eDribble_HandGuard;
	static MotorToCAN sMotorToCAN;
	static ShootCMD sShootCMD;
	static uint8_t sDribble_TwoCount = 0;
	static uint8_t sDribble_Defense  = 0;
	
	
	Defense.pf->speedMode(&Defense, -60, 40);
	osDelay(1000);
	Defense.pf->setOrigin(&Defense);
	
	for(;;)
	{
	#if DRIBBLE_ENABLE
		switch(sDribbleCMD)
		{	
			if (gIfDrible)		//如果爪子能正常运球.........................
			{
				case eDribble_One:
					if (gDribble_OneCount == 0)
					{
						//云梯走到运球位置
						EscaladeL.pf->posMode(&EscaladeL, cEscaladeDribbleAngleL, cDJISpeedDutyLimit, cDJICurrentDutyLimit);
						osDelay(1);
						EscaladeR.pf->posMode(&EscaladeR, cEscaladeDribbleAngleR, cDJISpeedDutyLimit, cDJICurrentDutyLimit);
						osDelay(1);
						openClaw(osDelay,1);				//打开爪子
						osDelay(1);
						
						while ((ABS(EscaladeL.status.posation-cEscaladeDribbleAngleL) > 5.f) || 
								(ABS(EscaladeR.status.posation-cEscaladeDribbleAngleR) > 5.f))
						{						//等待云梯到达接球位置
							osDelay(1);
						}
						
						sShootCMD.Mode = eShoot_Catch;		//发出指令让推射电机到接球位置 防止挡到球下落
						xQueueSend(Shoot_CMDHandle,&sShootCMD,portMAX_DELAY);
						gDribble_OneCount = 1;
						sDribble_TwoCount = 0;
					} else 
					{
						sShootCMD.Mode = eShoot_Bottom; // 发出指令让推射电机到底部
						xQueueSend(Shoot_CMDHandle, &sShootCMD, portMAX_DELAY);
						
						while (gShootNowAngle > 200.f)
						{
							osDelay(1);
						}
						
						EscaladeL.pf->posMode(&EscaladeL, cEscaladeClawAngleL, 70, 60);
						osDelay(1);
						EscaladeR.pf->posMode(&EscaladeR, cEscaladeClawAngleR, 70, 60);
						osDelay(1);
						
						
	//					osDelay(500);

						while ((ABS(EscaladeL.status.posation-cEscaladeClawAngleL) > 5.f) || 
								(ABS(EscaladeR.status.posation-cEscaladeClawAngleR) > 5.f))
						{							//等待云梯到达接球位置
							osDelay(1);
						}
						
						bigOpenClaw(osDelay,1);		//爪子大大的打开
						osDelay(1);
						
						while ((ABS(ClawL.status.posation-cClawBigOpenAngleL) > 5.f) || 
								(ABS(ClawR.status.posation-cClawBigOpenAngleR) > 5.f))
						{						
							osDelay(1);
						}
						
						osDelay(500);
						
						EscaladeL.pf->posMode(&EscaladeL, cEscaladeShootWaitAngleL, cDJISpeedDutyLimit * 2, cDJICurrentDutyLimit * 2);
						osDelay(1);
						EscaladeR.pf->posMode(&EscaladeR, cEscaladeShootWaitAngleR, cDJISpeedDutyLimit * 2, cDJICurrentDutyLimit * 2);
						osDelay(1);
						gDribble_OneCount = 0;
					}
					
					break;
					
				case eDribble_Two:
					if (gDribble_OneCount == 0)
					{
						break;
					}
					
					if (sDribble_TwoCount == 0)
					{
						closeClaw(osDelay,1);		//合上爪子
						sDribble_TwoCount = 1;
					} else
					{
						pushCylinder();				//打出气缸
						osDelay(5);
						openClaw(osDelay,1);		//打开爪子
						osDelay(200);
						pullCylinder();				//收回气缸
						osDelay(400);
						if (sDribble_TwoCount == 1)
						{
							gClawAimAngle = cClawCloseAngle;
							gClawDeathWaitTime = xTaskGetTickCount();
							closeClaw(osDelay,1);	//合上爪子
							
						} else 
						{
							gClawAimAngle = cClawHalfCloseAngle;
							gClawDeathWaitTime = xTaskGetTickCount();
							closeHalfClaw(osDelay,1);
						}
						sDribble_TwoCount++;
					}	
					
					break;

				case eDribble_HandGuard:
					if (sDribble_Defense == 0)
					{
						Defense.pf->posMode(&Defense, 200, 40, 50);
						sDribble_Defense = 1;
					} else 
					{
						Defense.pf->posMode(&Defense, 20, 40, 50);
						sDribble_Defense = 0;
					}						
					
					break;
				case eDribble_Defense:				
					EscaladeL.pf->posMode(&EscaladeL, cEscaladeShootWaitAngleL, cDJISpeedDutyLimit / 3, cDJICurrentDutyLimit);
					osDelay(1);
					EscaladeR.pf->posMode(&EscaladeR, cEscaladeShootWaitAngleR, cDJISpeedDutyLimit / 3, cDJICurrentDutyLimit);
					osDelay(1);

					openClaw(osDelay,1);						//打开爪子
					osDelay(1);

					while ((ABS(EscaladeL.status.posation-cEscaladeShootWaitAngleL) > 5.f) || 
						(ABS(EscaladeR.status.posation-cEscaladeShootWaitAngleR) > 5.f))
					{											//等待云梯到达推射位置
						osDelay(1);
					}

					// 发出指令让推射电机到达防守位置
					sShootCMD.Mode = eShoot_Defense; 
					xQueueSend(Shoot_CMDHandle, &sShootCMD, portMAX_DELAY);
					
					// 等待推射电机到达防守位置
					while(ABS(gShootNowAngle - cShootDefenseAngle) > cDeathZone)
					{
						osDelay(1);
					}
					
					// 云梯回底部
					EscaladeL.pf->posMode(&EscaladeL, cEscaladeButtonAngleL, cDJISpeedDutyLimit, cDJICurrentDutyLimit);
					osDelay(1);
					EscaladeR.pf->posMode(&EscaladeR, cEscaladeButtonAngleR, cDJISpeedDutyLimit, cDJICurrentDutyLimit);
					osDelay(1);

					closeClaw(osDelay,1);				//合上爪子
					osDelay(1);

					break;

				default:	break;	
			}
		}
	#endif
		osDelay(1);
		xQueueReceive(Dribble_CMDHandle,&sDribbleCMD,portMAX_DELAY);
	}
}

/* 推射任务 ***************************************************************/

void shootTask(void *argument)
{
	static uint8_t encFasle20 = 0;
	static ShootCMD sShootCMD;
	sShootCMD.Mode = eShoot_Catch;
	sShootCMD.Duty = 0;
	
	for(;;)
	{
	#if SHOOT_ENABLE
		clutchUnBreak();
		osDelay(100);

		xQueueReceive(Shoot_CMDHandle,&sShootCMD,portMAX_DELAY);
		
		switch(sShootCMD.Mode)
		{		
			case eShoot_Bottom:
				if (gIfDrible)		//如果爪子能正常运球.........................
				{
					if ((EscaladeL.status.posation > cEscaladeShootWaitAngleL + 5.f) || 
						(EscaladeR.status.posation < cEscaladeShootWaitAngleR - 5.f))
					{
						EscaladeL.pf->posMode(&EscaladeL, cEscaladeShootWaitAngleL, cDJISpeedDutyLimit / 3, cDJICurrentDutyLimit);
						osDelay(1);
						EscaladeR.pf->posMode(&EscaladeR, cEscaladeShootWaitAngleR, cDJISpeedDutyLimit / 3, cDJICurrentDutyLimit);
						osDelay(1);

						openClaw(osDelay,1);						//打开爪子
						osDelay(1);

						while ((ABS(EscaladeL.status.posation-cEscaladeShootWaitAngleL) > 5.f) || 
							(ABS(EscaladeR.status.posation-cEscaladeShootWaitAngleR) > 5.f))
						{											//等待云梯到达推射位置
							osDelay(1);
						}
					}
				}
				
				while(1)
				{
					if (gShootNowAngle > 700.f)
					{
						Shoot.pf->dutyMode(&Shoot, -15.f);
					}
					else if ((gShootNowAngle > 200.f) && (gShootNowAngle <= 700.f))
					{
						Shoot.pf->dutyMode(&Shoot, -10.f);
					}
					else if ((gShootNowAngle > 10.f) && (gShootNowAngle <= 300.f))
					{
						Shoot.pf->dutyMode(&Shoot, -5.f);
					}
					else
					{
						Shoot.pf->dutyMode(&Shoot, 0);
						clutchBreak();
						osDelay(200);
						clutchUnBreak();
						break;
					}
					osDelay(1);
				}
				
				if (gIfDrible)		//如果爪子能正常运球.........................
				{
					if ((ABS(EscaladeL.status.posation-cEscaladeDribbleAngleL) < 5.f) || 
						(ABS(EscaladeR.status.posation-cEscaladeDribbleAngleR) < 5.f))
					{						
						openClaw(osDelay,1);
						
						while ((ABS(ClawL.status.posation-cClawOpenAngleL) > 5.f) || 
								(ABS(ClawR.status.posation-cClawOpenAngleR) > 5.f))
						{						
							osDelay(1);
						}
						
						EscaladeL.pf->posMode(&EscaladeL, cEscaladeShootWaitAngleL, cDJISpeedDutyLimit * 2, cDJICurrentDutyLimit * 2);
						osDelay(1);
						EscaladeR.pf->posMode(&EscaladeR, cEscaladeShootWaitAngleR, cDJISpeedDutyLimit * 2, cDJICurrentDutyLimit * 2);
						osDelay(1);
						gDribble_OneCount = 0;
					}
				}
				
				break;
				
			case eShoot_Catch:
				while(1)
				{
					if ((gShootNowAngle - cCatchAngle) > cDeathZone)
					{
						Shoot.pf->dutyMode(&Shoot, -20.f);
					}
					else if ((gShootNowAngle - cCatchAngle) < (-cDeathZone))
					{
						Shoot.pf->dutyMode(&Shoot, 20.f);
					}
					else
					{
						Shoot.pf->dutyMode(&Shoot, 0);
						clutchBreak();
						osDelay(200);
						clutchUnBreak();
						break;
					}
					osDelay(1);
				}
				break;
				
				
			case eShoot_Push:
				if (gIfDrible)		//如果爪子能正常运球.........................
				{
					if ((ABS(ClawL.status.posation-cClawOpenAngleL) > 5.f) || 
							(ABS(ClawR.status.posation-cClawOpenAngleR) > 5.f))
					{						
						break;
					}
					
					if ((ABS(EscaladeL.status.posation-cEscaladeShootWaitAngleL) > 5.f) || 
						(ABS(EscaladeR.status.posation-cEscaladeShootWaitAngleR) > 5.f))
					{						
						break;
					}
				}
				
				if (gShootNowAngle > 12.f)
				{
					break;
				}
				
				while(1)
				{       
					static float sLastAngle = 0;
								
					if (sLastAngle == gShootNowAngle)
					{
						encFasle20++;
						if (encFasle20 >= 20)
						{
							Shoot.pf->dutyMode(&Shoot, 0);
							clutchBreak();      			//刹车
							osDelay(500);								//刹车时间, 暂不改
							clutchUnBreak();							//解刹车
							osDelay(500);								//取消刹车时间, 确保刹车取消, 暂不改
							//break之前之前总延时必须超过1000ms确保电机解刹车
							gAim_Sign = 0;
							break;
						}
					} else 
					{
						encFasle20 = 0;
					}
					
					
					if (gShootNowAngle < cShootAngle)
					{
						Shoot.pf->dutyMode(&Shoot, DEBUGMODE);
					} 					
					else
					{
						Shoot.pf->dutyMode(&Shoot, 0);
						clutchBreak();      						//刹车
						osDelay(500);								//刹车时间, 暂不改
						clutchUnBreak();							//解刹车
						osDelay(500);								//取消刹车时间, 确保刹车取消, 暂不改
						//break之前之前总延时必须超过1000ms确保电机解刹车
						gAim_Sign = 0;
						
						pullShootCylinder();
						break;
					}
					sLastAngle = gShootNowAngle;
					osDelay(1);
				}
				break;
				
			case eShoot_Defense:
				while(1)
				{
					if ((gShootNowAngle - cShootDefenseAngle) > cDeathZone)
					{
						Shoot.pf->dutyMode(&Shoot, -10.f);
					}
					else if ((gShootNowAngle - cShootDefenseAngle) < (-cDeathZone))
					{
						Shoot.pf->dutyMode(&Shoot, 10.f);
					}
					else
					{
						Shoot.pf->dutyMode(&Shoot, 0);
						clutchBreak();
						osDelay(200);
						clutchUnBreak();
						break;
					}
					osDelay(1);
				}
				
			default:	break;	
		}
	
	#endif	
		xSemaphoreGive(ShootFinishHandle);
		osDelay(1);
	}
}
